#include <errno.h>
#include <pthread.h>
#include <fcntl.h>
#include <sys/epoll.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <math.h>
#include <time.h>
#include <strings.h>

#include <cutils/log.h>
#include <cutils/sockets.h>
#include <hardware/gps.h>
#include <cutils/properties.h>
#include "platform_config.h"
#include "myjni.h"
#ifdef LOG_TAG
#undef LOG_TAG
#define LOG_TAG "gprsService"
#endif
#define GPS_DEBUG  0
#define NMEA_DEBUG 0
#define GPS_SV_INCLUDE 1
#include <linux/ioctl.h>
#include <linux/rtc.h>
#include <utils/Atomic.h>
#include <linux/android_alarm.h>

extern SysInf  SysInfor;

typedef enum {
    GPS_SV = 0,
    BDS_SV = 1,
    GLONASS_SV = 2
}SV_TYPE;

extern int threadRuning;
pthread_mutex_t mutex_gpsdata_lock = PTHREAD_MUTEX_INITIALIZER;
_GPSDATAREG   gpsdata;
#define PRN_PLUS_BDS 200
#define PRN_PLUS_GLN 64

#define GNSS_SPEED B9600
#if GPS_DEBUG
#  define  D(f, ...)   LOGD("%s: line = %d, " f, __func__, __LINE__, ##__VA_ARGS__)
#else
#  define  D(...)   ((void)0)
#endif

/*****************************************************************/
/*****************************************************************/
/*****                                                       *****/
/*****       N M E A   T O K E N I Z E R                     *****/
/*****                                                       *****/
/*****************************************************************/
/*****************************************************************/

typedef struct {
    const char*  p;
    const char*  end;
} Token;
volatile int gps_state = 0;
#define  MAX_NMEA_TOKENS  32

typedef struct {
    int     count;
    Token   tokens[ MAX_NMEA_TOKENS ];
} NmeaTokenizer;


static int nmea_tokenizer_init( NmeaTokenizer*  t, const char*  p, const char*  end ) {
    int    count = 0;
    char*  q; 

    // the initial '$' is optional
    if (p < end && p[0] == '$')
        p += 1;

    // remove trailing newline
    if (end > p && end[-1] == '\n') {
        end -= 1;
        if (end > p && end[-1] == '\r')
            end -= 1;
    }

    // get rid of checksum at the end of the sentecne
    if (end >= p+3 && end[-3] == '*') {
        end -= 3;
    }

    while (p < end) {
        const char*  q = p;

        q = memchr(p, ',', end-p);
        if (q == NULL)
            q = end;
        if (q >= p) {
            if (count < MAX_NMEA_TOKENS) {
                t->tokens[count].p   = p;
                t->tokens[count].end = q;
                count += 1;
            }
        }
        if (q < end)
            q += 1;

        p = q;
    }

    t->count = count;
    return count;
}

static Token nmea_tokenizer_get( NmeaTokenizer*  t, int  index ) {
    Token  tok;
    static const char*  dummy = "";

    if (index < 0 || index >= t->count) {
        tok.p = tok.end = dummy;
    } else
        tok = t->tokens[index];

    return tok;
}


static int str2int( const char*  p, const char*  end ) {
    int   result = 0;
    int   len    = end - p;

    for ( ; len > 0; len--, p++ )
    {
        int  c;

        if (p >= end)
            goto Fail;

        c = *p - '0';
        if ((unsigned)c >= 10)
            goto Fail;

        result = result*10 + c;
    }
    return  result;

Fail:
    return -1;
}

static double str2float( const char*  p, const char*  end )
{
    int   result = 0;
    int   len    = end - p;
    char  temp[16];

    if (len >= (int)sizeof(temp))
        return 0.;

    memcpy( temp, p, len );
    temp[len] = 0;
    return strtod( temp, NULL );
}

/*****************************************************************/
/*****************************************************************/
/*****                                                       *****/
/*****       N M E A   P A R S E R                           *****/
/*****                                                       *****/
/*****************************************************************/
/*****************************************************************/

#define  NMEA_MAX_SIZE  83
#define  MAX_SV_PRN 256
typedef struct {
    int     pos;
    int     overflow;
    int     utc_year;
    int     utc_mon;
    int     utc_day;
    int     utc_diff;
    GpsLocation  fix;
    GpsStatus status;
#if GPS_SV_INCLUDE
    GpsSvStatus  sv_status; 
    int		sv_num;
    int     sv_status_changed;
    int		sv_used_in_fix[MAX_SV_PRN];
#endif
    gps_location_callback  callback;
    gps_nmea_callback nmea_callback;
    gps_status_callback status_callback;
#if GPS_SV_INCLUDE
    gps_sv_status_callback sv_callback;
#endif
    char    in[ NMEA_MAX_SIZE+1 ];
} NmeaReader;


static void nmea_reader_update_utc_diff( NmeaReader*  r )
{
    time_t         now = time(NULL);
    struct tm      tm_local;
    struct tm      tm_utc;
    long           time_local, time_utc;

    gmtime_r( &now, &tm_utc );
    localtime_r( &now, &tm_local );

    time_local = tm_local.tm_sec +
        60*(tm_local.tm_min +
                60*(tm_local.tm_hour +
                    24*(tm_local.tm_yday +
                        365*tm_local.tm_year)));

    time_utc = tm_utc.tm_sec +
        60*(tm_utc.tm_min +
                60*(tm_utc.tm_hour +
                    24*(tm_utc.tm_yday +
                        365*tm_utc.tm_year)));

    r->utc_diff = time_local - time_utc;
}


static void nmea_reader_init( NmeaReader*  r )
{
    memset( r, 0, sizeof(*r) );

    r->pos      = 0;
    r->overflow = 0;
    r->utc_year = -1;
    r->utc_mon  = -1;
    r->utc_day  = -1;
    r->callback = NULL;
    r->nmea_callback = NULL;
    r->status_callback = NULL;
    r->sv_status.num_svs = 0;
    r->fix.size = sizeof(GpsLocation);

    nmea_reader_update_utc_diff( r );
}



int gps_year = 0;
int gps_mon = 0;
int gps_day = 0;
int gps_hour = 0;
int gps_min = 0;
int gps_sec = 0;
static int nmea_reader_update_time( NmeaReader*  r, Token  tok, struct tm *t )
{
    int        hour, minute;
    double     seconds;
    struct tm  tm, *ptm;
    time_t     fix_time;

    if (tok.p + 6 > tok.end)
        return -1;

    if (r->utc_year < 0) {
        // no date yet, get current one
        time_t  now = time(NULL);
        gmtime_r( &now, &tm );
        r->utc_year = tm.tm_year + 1900;
        r->utc_mon  = tm.tm_mon + 1;
        r->utc_day  = tm.tm_mday;
    }

    hour    = str2int(tok.p,   tok.p+2);
    minute  = str2int(tok.p+2, tok.p+4);
    seconds = str2float(tok.p+4, tok.end);

    tm.tm_hour  = hour;
    tm.tm_min   = minute;
    tm.tm_sec   = (int) seconds;
    tm.tm_year  = r->utc_year - 1900;
    tm.tm_mon   = r->utc_mon - 1;
    tm.tm_mday  = r->utc_day;
    tm.tm_isdst = -1;

	gps_year = r->utc_year;
	gps_mon = r->utc_mon;
	gps_day = r->utc_day;
	gps_hour = hour;
	gps_min = minute;
	gps_sec = seconds;


    fix_time = mktime( &tm ) + r->utc_diff;
    ptm = localtime(&fix_time);
    *t = *ptm;
    r->fix.timestamp = (long long)fix_time * 1000;
    return 0;
}

static int nmea_reader_update_date( NmeaReader*  r, Token  date, Token  time , struct tm *tm)
{
    Token  tok = date;
    int    day, mon, year;

    if (tok.p + 6 != tok.end) {
        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    day  = str2int(tok.p, tok.p+2);
    mon  = str2int(tok.p+2, tok.p+4);
    year = str2int(tok.p+4, tok.p+6) + 2000;

    if ((day|mon|year) < 0) {
        D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }

    r->utc_year  = year;
    r->utc_mon   = mon;
    r->utc_day   = day;

    return nmea_reader_update_time( r, time, tm);
}


static double convert_from_hhmm( Token  tok )
{
    double  val     = str2float(tok.p, tok.end);
    int     degrees = (int)(floor(val) / 100);
    double  minutes = val - degrees * 100.;
    double  dcoord  = degrees + minutes / 60.0;
    return dcoord;
}
double gps_speed = 0, gps_bearing = 0;
static unsigned int writeGpsInfoCount = 0;
extern volatile unsigned int start_deep_work, depth;
static int nmea_reader_update_latlong( NmeaReader*  r,
        Token        latitude,
        char         latitudeHemi,
        Token        longitude,
        char         longitudeHemi, 
        struct tm *p )
{
    double   lat, lon;
    Token    tok;
    char gps_str[256];

    tok = latitude;
    if (tok.p + 6 > tok.end) {
        D("latitude is too short: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    lat = convert_from_hhmm(tok);
    if (latitudeHemi == 'S')
        lat = -lat;

    tok = longitude;
    if (tok.p + 6 > tok.end) {
        D("longitude is too short: '%.*s'", tok.end-tok.p, tok.p);
        return -1;
    }
    lon = convert_from_hhmm(tok);
    if (longitudeHemi == 'W')
        lon = -lon;

    r->fix.flags    |= GPS_LOCATION_HAS_LAT_LONG;
    r->fix.latitude  = lat;
    r->fix.longitude = lon;
    //LOGE("lat=%f, lon=%f", lat, lon);
    cur_lat = lat;
    cur_lon = lon;
    AndroidGpsProcess(lat, lon, start_deep_work, gps_bearing, gps_speed);
        sprintf(gps_str, "%d-%02d-%02d %02d:%02d:%02d    %f    %f", 
                (1900+p->tm_year), 1+p->tm_mon, p->tm_mday, p->tm_hour, p->tm_min, p->tm_sec, lat, lon);
		if (writeGpsInfoCount % 10 == 0) {
			write_gps_info("%s", gps_str);
			write_gps_area_info("%s", gps_str);
		}
    writeGpsInfoCount++;
    return 0;
}


static int nmea_reader_update_altitude( NmeaReader*  r,
        Token        altitude,
        Token        units )
{
    double  alt;
    Token   tok = altitude;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags   |= GPS_LOCATION_HAS_ALTITUDE;
    r->fix.altitude = str2float(tok.p, tok.end);
    cur_altitude = r->fix.altitude;  
    return 0;
}

static int nmea_reader_update_accuracy( NmeaReader*  r,
        Token        accuracy )
{
    double  acc;
    Token   tok = accuracy;

    if (tok.p >= tok.end)
        return -1;

    r->fix.accuracy = str2float(tok.p, tok.end);

    if (r->fix.accuracy == 99.99){
        return 0;
    }

    r->fix.flags   |= GPS_LOCATION_HAS_ACCURACY;
    return 0;
}

static int nmea_reader_update_bearing( NmeaReader*  r,
        Token        bearing )
{
    double  alt;
    Token   tok = bearing;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags   |= GPS_LOCATION_HAS_BEARING;
    r->fix.bearing  = str2float(tok.p, tok.end);
    gps_bearing = r->fix.bearing;
    return 0;
}


static int nmea_reader_update_speed( NmeaReader*  r,
        Token        speed )
{
    double  alt;
    Token   tok = speed;

    if (tok.p >= tok.end)
        return -1;

    r->fix.flags   |= GPS_LOCATION_HAS_SPEED;
    gps_speed = str2float(tok.p, tok.end) * 1.852;//gps数据的速率单位为节, 1节等于每小时1海里,也就是每小时行驶1.852千米(公里);
    r->fix.speed    = gps_speed/3.6;//android的speed单位是 米/秒，所以要除以3.6，具体看hardhard gps.h定义解释
    return 0;
}

static int add_prn_plus(int prn, SV_TYPE sv_type) {    // add prn plus
    if (sv_type == BDS_SV && prn < PRN_PLUS_BDS)    
        prn += PRN_PLUS_BDS;
    else if(sv_type == GLONASS_SV && prn < PRN_PLUS_GLN)
        prn += PRN_PLUS_GLN;    
    return prn;
}


static void nmea_reader_encode_sv_status(NmeaReader*  r) {    // encode used_in_fix flag
    int i;

    // if num_svs is larger than GPS_MAX_SVS, set num_svs to GPS_MAX_SVS
    if ( r->sv_status.num_svs > GPS_MAX_SVS)     
        r->sv_status.num_svs = GPS_MAX_SVS;      // this will prevent overflow crash
    for ( i = 0; i < r->sv_status.num_svs; ++i)
    {
        GpsSvInfo *info = &(r->sv_status.sv_list[i]);
        int prn = info->prn;
        info->azimuth = (int)info->azimuth;	
        if (r->sv_used_in_fix[prn]) {
            info->azimuth += 720; 
        }
    }
}

static int setCurrentTimeMillis(int64_t millis)
{
    struct timeval tv; 
    struct timespec ts; 
    int fd; 
    int res;
    int ret = 0;

    if (millis <= 0 || millis / 1000LL >= INT_MAX) {
        return -1; 
    }   

    tv.tv_sec = (time_t) (millis / 1000LL);
    tv.tv_usec = (suseconds_t) ((millis % 1000LL) * 1000LL);

    fd = open("/dev/alarm", O_RDWR);
    if(fd < 0) {
        LOGE("Unable to open alarm driver: %s\n", strerror(errno));
        return -1; 
    }   
    ts.tv_sec = tv.tv_sec;
    ts.tv_nsec = tv.tv_usec * 1000;
    res = ioctl(fd, ANDROID_ALARM_SET_RTC, &ts);
    if(res < 0) {
        LOGE("Unable to set rtc to %ld: %s\n", tv.tv_sec, strerror(errno));
        ret = -1; 
    }   
    close(fd);
    LOGE("设置系统时间成功");
    return ret;

}

static int checkCsResult(char *p) {
    int i = 1;
    char result = 0;
    char *p_end;
    char buf[3] = {0};

    if (!p)
        return -1;

    p_end = strchr(p, '*');
    if (!p_end)
        return -1;

    while (p[i] != '*') {
        result ^= p[i];
        i++;
    }

    snprintf(buf, 3, "%02X", result);
    if (memcmp(buf, p_end + 1, 2))
        return -1;
    //LOGE("%s ok", __func__); 
    return 0;
}


static void nmea_reader_parse( NmeaReader*  r )
{
    /* we received a complete sentence, now parse it to generate
     * a new GPS fix...
     */
    NmeaTokenizer  tzer[1];
    Token          tok;
    int			   sv_type;

    D("Received: '%.*s'", r->pos, r->in);
    if (r->pos < 9) {
        D("Too short. discarded.");
        return;
    }

    nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if NMEA_DEBUG
    {
        int  n;
        D("Found %d tokens", tzer->count);
        for (n = 0; n < tzer->count; n++) {
            Token  tok = nmea_tokenizer_get(tzer,n);
            D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
        }
    }
#endif

    tok = nmea_tokenizer_get(tzer, 0);
    if (tok.p + 5 > tok.end) {
        D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
        return;
    }    

    if (memcmp(tok.p, "BD", 2) == 0) {
        sv_type = BDS_SV;
        D("BDS satellites");
    }
    else if (memcmp(tok.p, "GL", 2) == 0 ) {
        sv_type = GLONASS_SV;
        D("GLONASS satellites");
    }
    else {
        sv_type = GPS_SV;
        D("GPS satellites");
    }	
    // ignore first two characters.
    tok.p += 2;
    if ( !memcmp(tok.p, "GGA", 3) ) {
        // GPS fix
        Token  tok_time          = nmea_tokenizer_get(tzer,1);
        Token  tok_latitude      = nmea_tokenizer_get(tzer,2);
        Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,3);
        Token  tok_longitude     = nmea_tokenizer_get(tzer,4);
        Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
        Token  tok_isPix         = nmea_tokenizer_get(tzer,6);
        Token  tok_altitude      = nmea_tokenizer_get(tzer,9);
        Token  tok_altitudeUnits = nmea_tokenizer_get(tzer,10);

        if (tok_isPix.p[0] == '1') {
            //nmea_reader_update_time(r, tok_time);
            //nmea_reader_update_latlong(r, tok_latitude,
            //        tok_latitudeHemi.p[0],
            //        tok_longitude,
            //        tok_longitudeHemi.p[0]);
            nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
        }
        //memset(r->sv_used_in_fix, 0, sizeof(MAX_SV_PRN));	
    }  
#if 0
    else if ( !memcmp(tok.p, "GSA", 3) ) {
#if GPS_SV_INCLUDE

        Token  tok_fixStatus   = nmea_tokenizer_get(tzer, 2);
        int i;

        if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {

            Token  tok_accuracy      = nmea_tokenizer_get(tzer, 15);

            nmea_reader_update_accuracy(r, tok_accuracy);   // pdop

            for (i = 3; i <= 14; ++i) {

                Token  tok_prn  = nmea_tokenizer_get(tzer, i);
                int prn = add_prn_plus(str2int(tok_prn.p, tok_prn.end), sv_type);
                if (prn > 0 && prn < MAX_SV_PRN)
                    r->sv_used_in_fix[prn] = 1;
            }

        }
#endif
        // do something ?
    } else
#endif
    else if ( !memcmp(tok.p, "RMC", 3) ) {
            Token  tok_time          = nmea_tokenizer_get(tzer,1);
            Token  tok_fixStatus     = nmea_tokenizer_get(tzer,2);
            Token  tok_latitude      = nmea_tokenizer_get(tzer,3);
            Token  tok_latitudeHemi  = nmea_tokenizer_get(tzer,4);
            Token  tok_longitude     = nmea_tokenizer_get(tzer,5);
            Token  tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
            Token  tok_speed         = nmea_tokenizer_get(tzer,7);
            Token  tok_bearing       = nmea_tokenizer_get(tzer,8);
            Token  tok_date          = nmea_tokenizer_get(tzer,9);


            if (checkCsResult(r->in))
                return;
            pthread_mutex_lock(&mutex_gpsdata_lock);
            strcpy((char *)gpsdata.gps_data, r->in);
            pthread_mutex_unlock(&mutex_gpsdata_lock);

            //LOGE("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
            if (tok_fixStatus.p[0] == 'A')
            {
                struct tm tm;
                bzero(&tm, sizeof(tm));
                int ret = nmea_reader_update_date( r, tok_date, tok_time, &tm );

                gpsdata.new_msg_flag = 1;
                if ((gps_state==0) && (ret==0)) {//未定位->定位，设置一次系统时间
#if 0
                     time_t timep;
                      struct timeval tv;
                     timep = mktime(&tm) + r->utc_diff;
                     tv.tv_sec = timep;  
                     tv.tv_usec = 0;  
                     if(settimeofday(&tv, (struct timezone *) 0) == 0)  
                         LOGE("设置系统时间成功");  
                     else
                         LOGE("设置系统时间错误");  
                     system("busybox hwclock -w");
#endif
                     setCurrentTimeMillis(r->fix.timestamp);
                }
                gps_state = 1;
                nmea_reader_update_latlong( r, tok_latitude,
                        tok_latitudeHemi.p[0],
                        tok_longitude,
                        tok_longitudeHemi.p[0], 
                        &tm);

                nmea_reader_update_bearing( r, tok_bearing );
                nmea_reader_update_speed  ( r, tok_speed );
            } else {
                gps_state = 0;
            }
#if GPS_SV_INCLUDE
            r->sv_status_changed = 1;   // update sv status when receive gps, that's last sv status.
#endif

        } else if ( !memcmp(tok.p, "GSV", 3) ) {
#if 0
            Token  tok_noSatellites  = nmea_tokenizer_get(tzer, 3);
            int    noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);

            if (noSatellites > 0) {

                Token  tok_noSentences   = nmea_tokenizer_get(tzer, 1);
                Token  tok_sentence      = nmea_tokenizer_get(tzer, 2);

                int sentence = str2int(tok_sentence.p, tok_sentence.end);
                int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);
                int curr;
                int i;


                if (sentence == 1) {
                    r->sv_num = 0;
                }

                curr = r->sv_status.num_svs;

                i = 0;
                // max 4 group sv info in one sentence
                while (i < 4 && r->sv_num < noSatellites){

                    Token  tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);
                    Token  tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);
                    Token  tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);
                    Token  tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);

                    if (curr >= 0 && curr < GPS_MAX_SVS) {  // prevent from overflow
                        r->sv_status.sv_list[curr].prn = add_prn_plus(str2int(tok_prn.p, tok_prn.end), sv_type);
                        r->sv_status.sv_list[curr].elevation = str2float(tok_elevation.p, tok_elevation.end);
                        r->sv_status.sv_list[curr].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
                        r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);
                    }
                    r->sv_status.num_svs += 1;
                    r->sv_num += 1;

                    curr += 1;

                    i += 1;
                }
                /*
                   if (sentence == totalSentences) {
                //r->sv_status_changed = 1;
                }
                */

                D("GSV message with total satellites %d", noSatellites);   

            }
#endif
        }else {
            tok.p -= 2;
            D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
        }
#if 0
    if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
        r->fix.flags |=GPS_LOCATION_HAS_SPEED;  
        r->fix.flags |=GPS_LOCATION_HAS_ACCURACY;  
        r->fix.flags |=GPS_LOCATION_HAS_BEARING;  
        r->fix.flags |=GPS_LOCATION_HAS_ALTITUDE;  
#if GPS_DEBUG
        char   temp[256];
        char*  p   = temp;
        char*  end = p + sizeof(temp);
        time_t time;
        struct tm   utc;

        p += snprintf( p, end-p, "sending fix" );
        if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
            p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
            p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
            p += snprintf(p, end-p, " speed=%g", r->fix.speed);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
            p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
        }
        if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
            p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
        }

        time = r->fix.timestamp / 1000;	
        gmtime_r( &time, &utc );
        p += snprintf(p, end-p, " time=%s", asctime( &utc ));
        D("%s", temp);
#endif
        if (r->callback) {
            r->callback( &r->fix );
            r->fix.flags = 0;
        }
        else {
            D("no callback, keeping data until needed !");
        }
        // if (r->status_callback) {
        //     r->status.status = GPS_STATUS_ENGINE_OFF;
        //     r->status_callback(&r->status);
        // }
    }
#if GPS_SV_INCLUDE
    if ( r->sv_status_changed == 1 ) {
        r->sv_status_changed = 0;
        if (r->sv_callback) {            
            nmea_reader_encode_sv_status(r);            
            r->sv_callback(&r->sv_status);

            r->sv_status.num_svs = 0;
            memset(r->sv_used_in_fix, 0, sizeof( MAX_SV_PRN ));	
        }
        else{
            D("no sv callback, keeping data until needed !");
        }
    }
#endif
#endif
}
/*
   static GpsUtcTime get_system_timestamp() {
   struct timeval tp;
   gettimeofday(&tp, NULL);    // get current time
   GpsUtcTime t = tp.tv_sec * 1000 + tp.tv_usec / 1000;
   return t;
   }
   */
static void nmea_reader_addc( NmeaReader*  r, int  c )
{
    if (r->overflow) {
        r->overflow = (c != '\n');
        return;
    }

    if (r->pos >= (int) sizeof(r->in)-1 ) {
        r->overflow = 1;
        r->pos      = 0;
        return;
    }

    r->in[r->pos] = (char)c;
    r->pos       += 1;

    if (c == '\n') {
        nmea_reader_parse( r );
        if (r->nmea_callback) {
            r->nmea_callback( r->fix.timestamp, r->in, r->pos );
        }
        else {
            D("No nmea callback");
        }
        r->pos = 0;
    }
}


/* this is the state of our connection to the qemu_gpsd daemon */
typedef struct {
    int                     init;
    int                     fd;
    GpsCallbacks            callbacks;
    pthread_t               thread;
    int                     control[2];
    char                    device[32];
    int                     speed;
} GpsState;

static GpsState  gpsState[1];

static int epoll_register( int  epoll_fd, int  fd )
{
    struct epoll_event  ev;
    int                 ret, flags;

    /* important: make the fd non-blocking */
    flags = fcntl(fd, F_GETFL);
    fcntl(fd, F_SETFL, flags | O_NONBLOCK);

    ev.events  = EPOLLIN;
    ev.data.fd = fd;
    do {
        ret = epoll_ctl( epoll_fd, EPOLL_CTL_ADD, fd, &ev );
    } while (ret < 0 && errno == EINTR);
    return ret;
}


static int epoll_deregister( int  epoll_fd, int  fd )
{
    int  ret;
    do {
        ret = epoll_ctl( epoll_fd, EPOLL_CTL_DEL, fd, NULL );
    } while (ret < 0 && errno == EINTR);
    return ret;
}

//梨具识别器
int ZL_HT23B_fd = -1;
int ZL_HT23B_init() {
    struct termios cfg;    
    struct termios termios;
    int fd = open("/dev/ttyS3", O_RDWR | O_NONBLOCK | O_NOCTTY);
    if (fd < 0) {
        LOGE("no gps detected");
        return -1;
    }
    tcgetattr(fd, &cfg);
    cfmakeraw(&cfg);
    cfsetispeed(&cfg, B115200);
    cfsetospeed(&cfg, B115200);
    tcsetattr(fd, TCSANOW, &cfg);
    return fd;
}



/* this is the main thread, it waits for commands from gps_state_start/stop and,
 * when started, messages from the QEMU GPS daemon. these are simple NMEA sentences
 * that must be parsed to be converted into GPS fixes sent to the framework
 */
static void gps_state_thread( void*  arg )
{
	long t1, t2;
    GpsState*   state = (GpsState*) arg;
    NmeaReader  reader[1];
    int         epoll_fd   = epoll_create(2);
    int         started    = 0;
    int         gps_fd     = state->fd;
    int         t_sec = -1;
    nmea_reader_init( reader );

    epoll_register(epoll_fd, gps_fd);
    if (ZL_HT23B_fd > 0)
        epoll_register(epoll_fd, ZL_HT23B_fd);

    D("gps thread running");

    // now loop
    while (threadRuning) {
        struct epoll_event   events[2];
        int                  ne, nevents;
        char  buff[128];

        memset(buff, 0, sizeof(buff));
        nevents = epoll_wait( epoll_fd, events, 1, 10000);
        if (nevents <= 0) {
            if (errno != EINTR){
                LOGE("epoll_wait() unexpected error: %s", strerror(errno));
				gps_state = 0;
			}
            continue;
        }
        D("gps thread received %d events", nevents);
        for (ne = 0; ne < nevents; ne++) {
            if ((events[ne].events & (EPOLLERR|EPOLLHUP)) != 0) {
                LOGE("EPOLLERR or EPOLLHUP after epoll_wait() !?");
                goto out;
            }
            int  fd = events[ne].data.fd;
            if (fd == gps_fd) {
                D("gps fd event");
                for (;;) {
                    int  nn, ret;

                    ret = read( fd, buff, sizeof( buff ) - 1 );
                    if (ret < 0) {
                        if (errno == EINTR)
                            continue;
                        if (errno != EWOULDBLOCK)
                            LOGE("error while reading from gps daemon socket: %s:", strerror(errno));
                        break;
                    }

                    D("gps fd received: %.*s bytes: %d", ret, buff, ret);
                    for (nn = 0; nn < ret; nn++)
                        nmea_reader_addc( reader, buff[nn] );
                }
                //LOGE("gps fd event end");
            } else if (fd == ZL_HT23B_fd) {
                int  nn, ret;
                ret = read(fd, buff, sizeof( buff ) - 1 );
                if (ret > 0) {
                    LOGE("ZL_HT23B data:%s", buff);
					if (strstr(buff, "@card:") != NULL)
					{
						if (strstr(buff, "@card:0000000000") == NULL && strstr(buff, SysInfor.plough_care_id) != NULL)
						{
							plough_state = 1;
							t1 = currentTimeInSec();
						}
						else
						{
							t2 = currentTimeInSec();
							if (t2 - t1 > 3600 * 3) //超过三个小时读取不到犁具识别器ID
							{
								plough_state = 0;
							}
						}
						LOGE("plough_state = %d", plough_state);
					}
					else if (strstr(buff, "@lowbat:") != NULL)
					{
						LOGE("犁具识别器电量低！");
					}
                }
            } else {
                LOGE("epoll_wait() returned unkown fd %d ?", fd);
            }
        }
    }
out:
    gps_state = 0;
    epoll_deregister(epoll_fd, gps_fd);
    if (ZL_HT23B_fd > 0) {
        epoll_deregister(epoll_fd, ZL_HT23B_fd);
        close(ZL_HT23B_fd);
    }
    close(epoll_fd);
    close(gps_fd);
    LOGE("%s退出", __func__);
}

void *gps_state_init(void *arg)
{
    struct termios termios;

    GpsState*  state = gpsState;
    state->init       = 1;
    state->fd         = -1; 



    strcpy(state->device, "/dev/ttyS4");
    state->speed = GNSS_SPEED;

    state->fd = open(state->device, O_RDWR | O_NONBLOCK | O_NOCTTY);

    if (state->fd < 0) {
        D("no gps detected");
        return NULL;
    }   
    D("gps uart open %s success!", state->device);

    struct termios cfg;    
    tcgetattr(state->fd, &cfg);
    cfmakeraw(&cfg);
    cfsetispeed(&cfg, state->speed);
    cfsetospeed(&cfg, state->speed);
    tcsetattr(state->fd, TCSANOW, &cfg);

    D("gps will read from %s", state->device);

    ZL_HT23B_fd = ZL_HT23B_init();

    gps_state_thread(state);

    return NULL;
}

int gps_factory_test(void) {
    struct termios cfg;    
    char buffer[1024];
    int result = -1;

    int fd = open("/dev/ttyS4", O_RDWR | O_NONBLOCK | O_NOCTTY);
    if (fd < 0) {
        LOGE("no gps detected");
        return -1;
    }
    tcgetattr(fd, &cfg);
    cfmakeraw(&cfg);
    cfsetispeed(&cfg, B9600);
    cfsetospeed(&cfg, B9600);
    tcsetattr(fd, TCSANOW, &cfg);
    fd_set fds; 
    struct timeval tv;
    int r;
    int try_count = 0;
    u32 len = 0;

    memset(buffer, 0, sizeof(buffer));
    while (try_count++ < 2) { 
        FD_ZERO (&fds);
        FD_SET(fd, &fds);
        tv.tv_sec = 4; 
        tv.tv_usec = 0; 

        r = select (fd+1, &fds, NULL, NULL, &tv);

        if (-1 == r) { 
            if (EINTR == errno) {
                LOGE("%s err=%s", __func__, strerror(errno));
                continue;
            }
        }
        if (0 == r) { 
            LOGE ("gps select timeout");
            continue;
        }
        if (FD_ISSET(fd, &fds)) {
            if (len >= sizeof(buffer))
                break;
			int ret = read(fd, buffer + len, sizeof(buffer) - len);
            if (ret > 0) { 
                len += ret;
                buffer[len] = 0;
                result = 0;
                break;

            }
        }
    }
out:
    close(fd);

    return result;
}
